/**
  ******************************************************************************
  * Copyright (c) 2019 - ~, SCUT-RobotLab Development Team
  * @file    PID.h
  * @author  BigeYoung 851756890@qq.com & M3chD09
  * @brief   PID controller sets. This file provides traditional PID controller 
  *			     and modern Fuzzy PID controller.
  ******************************************************************************
  * @attention
  * 
  * if you had modified this file, please make sure your code does not have any 
  * bugs, update the version Number, write dowm your name and the date. The most
  * important thing is make sure the users will have clear and definite under-
  * standing through your new brief.
  *
  * <h2><center>&copy; Copyright (c) 2019 - ~, SCUT-RobotLab Development Team.
  * All rights reserved.</center></h2>
  ******************************************************************************
  */
#pragma once

#ifdef __cplusplus
/* Includes ------------------------------------------------------------------*/
#include <stdint.h>
#include <stddef.h>
#include <limits.h>
#include "filters.h"   /* LowPassFilter() */
/* Private macros ------------------------------------------------------------*/
typedef uint32_t (*SystemTick_Fun)(void);
/* Private type --------------------------------------------------------------*/
/* Exported macros -----------------------------------------------------------*/
/* Exported types ------------------------------------------------------------*/
class myPIDTimer
{
public:
    static uint8_t getMicroTick_regist(uint32_t (*getTick_fun)(void));
                                            /*<! Regist get time function */
protected:
    static SystemTick_Fun Get_SystemTick;   /*<! Pointer of function to get system tick */
    float dt;				                        /*!< Differentiation of real time*/
    uint32_t last_time; 	                  /*!< Last recorded real time from systick*/
    uint8_t UpdataTimeStamp(void);                                 
};

/** 
* @brief Class for open loop control.
*/
class OpenLoop
{
public:
    OpenLoop(float gain = 1.0f) : Gain(gain) {}

    float Adjust()
    {
        Out = Gain * Target;
        return Out;
    }
    float Target, Now;
    float Out;

    float Gain;
};

/** 
* @brief Class for traditional PID control.
*/
class myPID : public myPIDTimer
{
public:
    myPID(float _Kp, float _Ki, float _Kd, float _I_Term_Max, float _Out_Max);
    float Adjust();

    // 积分清零
    void Integral_Reset() { integral_e = 0; };
    
    float Target, Now, Error;
    float Out;

    float Kp, Ki, Kd;
    float I_Term_Max;        /*<! I项限幅 */
    float Out_Max;           /*<! 输出限幅 */
	
	float I_Term;			/* 积分器输出 */
    float P_Term;			/* 比例器输出 */
    float D_Term;			/* 微分器输出 */

    float I_SeparThresh; // = ULONG_MAX;   /*!< 积分分离阈值，需为正数。fabs(error)大于该阈值取消积分作用。*/

    float VarSpeed_I_A;             // = ULONG_MAX; 
                                                    /*!< 变速积分 A，需为正数。*/
    float VarSpeed_I_B;             //= ULONG_MAX;
                                                    /*!< 变速积分 B，需为正数， \n
                                                    在 error<=B 的区间内，为普通积分效果， \n
                                                    在 B<error<=A+B 的区间内，为变速积分效果， \n
                                                    在 A+B<error 的区间内，不继续积分。*/

    float DeadZone; 		    /*!< 死区，需为整数，fabs(error)小于DeadZone时，输出为0。 */

    LowPassFilter LowPass_error;                    //= LowPassFilter(1);
    LowPassFilter LowPass_d_err;                    //= LowPassFilter(1); /*!< 不完全微分。 */

    bool D_of_Current; //= false; /*!< 启用微分先行，文献中Current多译作Process Variable(PV)。 */

private:
    float pre_error;
    float integral_e;

    float pre_Current;
};

/* Exported variables ---------------------------------------------------------*/
/* Exported function declarations ---------------------------------------------*/
#endif

/************************ COPYRIGHT(C) SCUT-ROBOTLAB **************************/
